www.gusucode.com > VC 3D弹道仿真程序源码文件-源码程序 > VC 3D弹道仿真程序源码文件-源码程序/code/ReferenceFrame.cpp
/********************************************************************************
**** 名称：坐标转换模块 *****
**** 编写：孙瑞胜 李伟明
**** 日期：2011年7月10日 *****
*********************************************************************************/
#include <math.h>
#include <stdio.h>
#include <malloc.h>
#include <fstream.h>
#include "./myInclude/myMatrix.h"
#include "ReferenceFrame.h"
#include "EarthModel.h"
#include "3DModel.h"
const double PI=3.1415926;
const double RAD=180./PI;
#define Rf 0.00335281317790 /*1.0/298.257*/ /*地球扁率*/
//Download by http://www.NewXing.com
extern double now_time ;
CFrameofRef::CFrameofRef()
{
}
CFrameofRef::~CFrameofRef()
{
}
/**************************************************************************/
/*** 名称：矩阵相乘 程序模块 ***/
/*** 功能：实现c[m][n]=a[m][p]*b[p][n] ***/
/**************************************************************************/
void CFrameofRef::MatrixMultiply(int m, int p, int n, const double *a, const double *b, double *c)
{
double temp=0;
for(int i=0; i<m; i++)
for(int j=0; j<n; j++)
{
temp=0;
for(int k=0; k<p; k++)
{
temp+=(a[p*i+k]) * (b[k*n+j]);
}
c[n*i+j]=temp;
}
return;
}
/**************************************************************************/
/*** 名称：矩阵转置 程序模块 ***/
/*** 功能：实现b[n][m]=a[m][n] ***/
/**************************************************************************/
void CFrameofRef::MatrixTrans(int m, int n, double *a, double *b)
{
int i,j;
for( i=0; i<m; i++)
{
for( j=0; j<n; j++)
{
b[j*m+i]=a[n*i+j];
}
}
return ;
}
/**********************************************************/
/*** 地面(发射)系到弹道系坐标转换程序模块 (LOH) ***/
/*** XS_l[3]为地面系中的分量,
XS_t[3]为弹道系中的分量,
pFlag='l2t' or 't2l' ***/
/**********************************************************/
void CFrameofRef::Ground2Traject( double theta, double Psi_V , double XS_l[3], double XS_t[3], const char* pFlag )
{
double Conversion_t2l[3][3], Conversion_l2t[3][3]= //地面(发射)系到弹道系坐标转换矩阵
{
cos( theta/RAD ) * cos( Psi_V/RAD ) , sin( theta/RAD ), -cos( theta/RAD ) * sin( Psi_V/RAD ) ,
-sin( theta/RAD ) * cos( Psi_V/RAD ), cos( theta/RAD ), sin( theta/RAD ) * sin( Psi_V/RAD ) ,
sin( Psi_V/RAD ) , 0 , cos( Psi_V/RAD )
};
if ( ( pFlag[0]== 'l' ) && ( pFlag[2]== 't' ) )
{
MatrixMultiply( 3, 3, 1, *Conversion_l2t, XS_l, XS_t );
}
else if ( ( pFlag[0]== 't' ) && ( pFlag[2]== 'l' ) )
{
MatrixTrans( 3, 3, *Conversion_l2t, *Conversion_t2l) ;
MatrixMultiply( 3, 3, 1, *Conversion_t2l, XS_l, XS_t );
}
else
{
cout<<"error: no fitable flag of options! options must be 'l2t or t2l' "<<endl ;
;
}
return ;
}
/**********************************************************/
/*** 弹道系到速度系坐标转换程序模块 (LHV) ***/
/*** XS_t[3]为弹道系中的分量, XS_v[3]速度系中的分量 ***/
/**********************************************************/
void CFrameofRef::Traject2Vel( double gama_v , double XS_t[3], double XS_v[3] )
{
double Conversion_t2v[3][3]= //弹道系到速度系坐标转换矩阵
{
1 , 0 , 0 ,
0 , cos( gama_v/RAD ) , sin( gama_v/RAD ) ,
0 , -sin( gama_v/RAD ), cos( gama_v/RAD )
};
MatrixMultiply( 3, 3, 1, *Conversion_t2v, XS_t, XS_v );
return ;
}
/**********************************************************/
/*** 速度系到弹体系坐标转换程序模块 (LVB) ***/
/*** XS_v[3]速度系中的分量 , XS_b[3]为弹体系中的分量 ***/
/**********************************************************/
void CFrameofRef::Vel2Body( double alpha, double beta , double XS_v[3], double XS_b[3] )
{
double Conversion_v2b[3][3]= //速度系到弹体系坐标转换矩阵
{
cos( alpha/RAD ) * cos( beta/RAD ) , sin( alpha/RAD ) , -cos( alpha/RAD ) * sin( beta/RAD ) ,
-sin( alpha/RAD ) * cos( beta/RAD ) , cos( alpha/RAD ) , sin( alpha/RAD ) * sin( beta/RAD ) ,
sin( beta/RAD ) , 0 , cos( beta/RAD )
};
MatrixMultiply( 3, 3, 1, *Conversion_v2b, XS_v, XS_b );
return ;
}
/**********************************************************/
/*** 地面(发射)系到弹体系坐标转换程序模块 (LOB) ***/
/*** XS_l[3]为地面系中的分量, XS_b[3]为弹体系中的分量 ***/
/**********************************************************/
void CFrameofRef::Ground2Body( double pitch, double yaw, double roll, double XS_l[3], double XS_b[3] )
{// CITA PESAI gamma
double Conversion_l2b[3][3]= //地面系到弹体系坐标转换矩阵
{
cos( pitch/RAD ) * cos( yaw/RAD ) , sin( pitch/RAD ) , -cos( pitch/RAD ) * sin( yaw/RAD ) ,
( -sin( pitch/RAD ) * cos( yaw/RAD ) * cos( roll/RAD ) + sin( yaw/RAD ) * sin( roll/RAD ) ) , cos( pitch/RAD ) * cos( roll/RAD ) , ( sin( pitch/RAD ) * sin( yaw/RAD ) * cos( roll/RAD ) + cos( yaw/RAD ) * sin( roll/RAD ) ) ,
( sin( pitch/RAD ) * cos( yaw/RAD ) * sin( roll/RAD ) + sin( yaw/RAD ) * cos( roll/RAD ) ) , -cos( pitch/RAD ) * sin( roll/RAD ) , ( -sin( pitch/RAD ) * sin( yaw/RAD ) * sin( roll/RAD ) + cos( yaw/RAD ) * cos( roll/RAD ) )
};
MatrixMultiply( 3, 3, 1, *Conversion_l2b, XS_l, XS_b );
return ;
}
/**********************************************************/
/*** 坐标经纬高到大地空间直角坐标系坐标转换程序模块 ***/
/*** XS_84[3]对应0纬 1经 2高, XS_ecef[3]对应0XE 1YE 2ZE ***/
/**********************************************************/
void CFrameofRef::WGS84_2_ECEF( double XS_84[3], double XS_ecef[3] )
{
double R = 6378140/sqrt( 1 - Rf*Rf*sin( XS_84[0]/RAD )*sin( XS_84[0]/RAD ) );
XS_ecef[0] = ( R+XS_84[2] ) * cos( XS_84[0]/RAD ) * cos( XS_84[1]/RAD );
XS_ecef[1] = ( R+XS_84[2] ) * cos( XS_84[0]/RAD ) * sin( XS_84[1]/RAD );
XS_ecef[2] = ( R*(1-Rf*Rf) + XS_84[2] ) * sin( XS_84[0]/RAD );
return;
}
/**********************************************************/
/*** 坐标经纬高到大地空间直角坐标系坐标转换程序模块 ***/
/*** XS_84[3]对应0纬 1经 2高, XS_ecef[3]对应0XE 1YE 2ZE ***/
/**********************************************************/
void CFrameofRef::ECEF_2_WGS84( double XS_ecef[3], double XS_84[3] )
{
double R ;
for (int i=0; i<4; i++ )
{
XS_84[0] = atan( ( XS_ecef[2]/sqrt( XS_ecef[0]*XS_ecef[0]+XS_ecef[1]*XS_ecef[1] ) )*( 1 + 6378140*Rf*Rf*sin( XS_84[0]/RAD )/( XS_ecef[2]*sqrt( 1-Rf*Rf*sin( XS_84[0] )*sin( XS_84[0]/RAD ) ) ) ) );
}
XS_84[0] = XS_84[0] * RAD ;
XS_84[1] = atan( XS_ecef[1]/XS_ecef[0] ) ;
R = 6378140/sqrt( 1 - Rf*Rf*sin( XS_84[0]/RAD )*sin( XS_84[0]/RAD ) );
XS_84[2] = sqrt( XS_ecef[0]*XS_ecef[0]+XS_ecef[1]*XS_ecef[1] )/cos( XS_84[0]/RAD ) - R ;
return;
}
/**************************************************************/
/*** 发射惯性坐标系到地面坐标系转换程序模块 (LAO ) ***/
/*** XS_i[3]为发射惯性系中的分量, XS_l[3]为地面系中的分量 ***/
/**************************************************************/
void CFrameofRef::Inertial2Ground( double XS_i[3], double XS_l[3] )
{
C3DModel * p3DModel_R = new C3DModel ;
CEarth * pEarth_R = new CEarth ;
double we_x, we_y, we_z ;
pEarth_R->Cal_we_xyz( p3DModel_R->B0, p3DModel_R->A0, we_x, we_y, we_z ) ;
double Conversion_i2l[3][3]= //发射惯性坐标系到地面坐标系转换矩阵
{
1 , we_z*now_time , -we_y*now_time ,
-we_z*now_time , 1 , we_x*now_time ,
we_y*now_time , -we_x*now_time , 1
};
MatrixMultiply( 3, 3, 1, *Conversion_i2l, XS_i, XS_l );
return;
}
/**********************************************************/
/*** 地面坐标系到导航（东北天）坐标系转换程序模块 (LON) ***/
/*** XS_l[3]为地面系中的分量, XS_n[3]为导航系中的分量 ***/
/**********************************************************/
void CFrameofRef::Ground2Nav( double alpha, double XS_l[3], double XS_n[3] )
{
double Conversion_l2n[3][3]= //地面坐标系到导航（东北天）坐标系转换矩阵
{
sin( alpha/RAD ) , 0 , cos( alpha/RAD ) ,
cos( alpha/RAD ) , 0 , -sin( alpha/RAD ) ,
0 , 1 , 0
};
MatrixMultiply( 3, 3, 1, *Conversion_l2n, XS_l, XS_n );
return;
}
/******************************************************************/
/*** 导航（东北天）到地面坐标系坐标系转换程序模块 (LNO) ***/
/*** XS_n[3]为导航系中的分量, XS_ecef[3]为地面坐标系中的分量 ***/
/******************************************************************/
void CFrameofRef::Nav2ECEF( double XS_n[3], double XS_ecef[3] )
{
// double Conversion_n2E[3][3]= //导航（东北天）到地面坐标系坐标系转换矩阵
{
};
// MatrixMultiply( 3, 3, 1, *Conversion_n2E, XS_n, XS_ecef );
return;
}
/*********************************************************************************************/
/*程序模块：目标系到导航系下的坐标转换 */
/*函数名：Lan_to_NavCoor() */
/*参数：alfa,Naviga,Launch_F */
/*返回：Loc 导航系下坐标值 */
/*作者： 胡锐 */
/*********************************************************************************************/
/*
Loc CTRANS::Lan_to_NavCoor( double alfa , Loc Launch_F )//方位角(弧度)、 发射系下坐标
{
Loc Naviga; //导航系下坐标
double cfn[3][3]= //发射系到导航系下的坐标转换矩阵
{
{ sin( alfa ), 0 , cos( alfa ) },
{ cos( alfa ), 0 , -sin( alfa ) },
{ 0 , 1 , 0 },
};
//发射系坐标转换到导航系
Naviga.x = cfn[0][0] * Launch_F.x + cfn[0][1] * Launch_F.y + cfn[0][2] * Launch_F.z ;
Naviga.y = cfn[1][0] * Launch_F.x + cfn[1][1] * Launch_F.y + cfn[1][2] * Launch_F.z ;
Naviga.z = cfn[2][0] * Launch_F.x + cfn[2][1] * Launch_F.y + cfn[2][2] * Launch_F.z ;
return Naviga ;
}
*/
/*********************************************************************************************/
/*程序模块：速度发射系到导航系下的转换 */
/*参数：alfa,Launch_F */
/*函数名：Lan_to_NavCoor_V() */
/*返回：Vel 导航系下速度 */
/*作者： 胡锐 */
/*********************************************************************************************/
/*
Vel CTRANS::Lan_to_NavCoor_V( double alfa , Vel Launch_V )//方位角(弧度)、发射系下速度
{
Vel Naviga_V; //导航系下坐标
double cfn[3][3]= //发射系到导航系下的坐标转换矩阵
{
{ sin( alfa ), 0 , cos( alfa ) },
{ cos( alfa ), 0 , -sin( alfa ) },
{ 0 , 1 , 0 },
};
//发射系坐标转换到导航系
Naviga_V.Vx = cfn[0][0] * Launch_V.Vx + cfn[0][1] * Launch_V.Vy + cfn[0][2] * Launch_V.Vz ;
Naviga_V.Vy = cfn[1][0] * Launch_V.Vx + cfn[1][1] * Launch_V.Vy + cfn[1][2] * Launch_V.Vz ;
Naviga_V.Vz = cfn[2][0] * Launch_V.Vx + cfn[2][1] * Launch_V.Vy + cfn[2][2] * Launch_V.Vz ;
return Naviga_V ;
}
*/
/*********************************************************************************************/
/*程序模块：速度发射系到导航系下的转换 */
/*函数名：Lan_to_NavCoor_V() */
/*参数：alfa,Launch_F */
/*返回：Vel 导航系下速度 */
/*作者： 胡锐 */
/*********************************************************************************************/
/*
Accel CTRANS::Launch_to_Bomb_Coor_A( Loc Angle , Accel Launch_A )//欧拉角(弧度)、发射系下加速度
{
Accel Bomb_A ;
double cfb[3][3] = //发射系到弹体系的转换矩阵
{
{ cos( Angle.x ) * cos( Angle.y ) , sin( Angle.x ) , -cos( Angle.x ) * sin( Angle.y ) },
{ -sin( Angle.x ) * cos( Angle.y ) * cos( Angle.z ) + sin( Angle.y ) * sin( Angle.z ) , cos( Angle.x ) * cos( Angle.z ) , sin( Angle.x ) * sin( Angle.y ) * cos( Angle.z ) + cos( Angle.y ) * sin( Angle.z ) },
{ sin( Angle.x ) * cos( Angle.y ) * sin( Angle.z ) + sin( Angle.y ) * cos( Angle.z ) , -cos( Angle.x ) * sin( Angle.z ) , -sin( Angle.x ) * sin( Angle.y ) * sin( Angle.z ) + cos( Angle.y ) * cos( Angle.z ) }
} ;
//发射系坐标转换到导航系
Bomb_A.Ax = cfb[0][0] * Launch_A.Ax + cfb[0][1] * Launch_A.Ay + cfb[0][2] * Launch_A.Az ;
Bomb_A.Ay = cfb[1][0] * Launch_A.Ax + cfb[1][1] * Launch_A.Ay + cfb[1][2] * Launch_A.Az ;
Bomb_A.Az = cfb[2][0] * Launch_A.Ax + cfb[2][1] * Launch_A.Ay + cfb[2][2] * Launch_A.Az ;
return Bomb_A ;
}
*/
/*********************************************************************************************/
/*程序模块：导航系增量到空间直角坐标系下的坐标增量转换 */
/*函数名：Nav_to_AngleCoor() */
/*参数：Delt_Naviga,Space_Point */
/*返回：Loc 空间直角坐标系下的增量输出 */
/*作者： 胡锐 */
/*********************************************************************************************/
/*
Loc CTRANS::Nav_to_AngleCoor( Loc Delt_Naviga , Gps Space_Point )//导航坐标增量,前点的经纬高(角度)
{
Loc Angle ;
double cne[3][3]= //导航系到大地系下的坐标转换矩阵
{
{ -sin( Space_Point.l / R_D ) , -sin( Space_Point.f / R_D ) * cos( Space_Point.l / R_D ) , cos( Space_Point.f / R_D ) * cos( Space_Point.l / R_D) },
{ cos( Space_Point.l / R_D ) , -sin( Space_Point.f / R_D ) * sin( Space_Point.l / R_D ) , cos( Space_Point.f / R_D ) * sin( Space_Point.l / R_D) },
{ 0 , cos( Space_Point.f / R_D ) , sin( Space_Point.f / R_D ) }
};
//导航增量转换到空间直角坐标增量
Angle.x = cne[0][0] * Delt_Naviga.x + cne[0][1] * Delt_Naviga.y + cne[0][2] * Delt_Naviga.z ;
Angle.y = cne[1][0] * Delt_Naviga.x + cne[1][1] * Delt_Naviga.y + cne[1][2] * Delt_Naviga.z ;
Angle.z = cne[2][0] * Delt_Naviga.x + cne[2][1] * Delt_Naviga.y + cne[2][2] * Delt_Naviga.z ;
return Angle ;
}
*/
/*********************************************************************************************/
/*程序模块：空间直角坐标系下的坐标到空间球坐标转换 */
/*函数名：Angle_to_TransitCoor() */
/*参数：Angle,Space_Point */
/*返回：Gps 84坐标系下的经纬高度坐标 */
/*作者： 胡锐 */
/*********************************************************************************************/
/*
Gps CTRANS::Angle_to_TransitCoor( Loc Angle , Gps Space_Point )//空间直角坐标,上一点经纬高(角度)
{
Gps Space ;
double temp1 , temp2 , temp , temp3 , N; //中间变量
temp2 = Space_Point.f / R_D ; //迭代法求发射点处纬度
temp1 = temp2 - 0.02 ;
temp3 = Angle.z / sqrt( Angle.x * Angle.x + Angle.y * Angle.y );
while ( fabs( temp2 - temp1 ) > EPSILON )
{
temp1 = temp2;
temp2 = atan( temp3 * ( 1 + R * e2 * sin( temp1 ) / Angle.z / sqrt( 1 -e2 * sin( temp1 ) * sin( temp1 ) ) ) );
} //迭代法解算出纬度(弧度)
Space.f = temp2 * R_D; //纬度值(角度)
temp= atan ( Angle.y / Angle.x ); //经度值(弧度)
if ( Space_Point.l > 45 && temp < 0 ) //解算出的经度弧度值得出应该的角度值
Space.l = temp * R_D + 180;
else if ( Space_Point.l < -45 && temp > 0)
Space.l = temp * R_D - 180;
else Space.l = temp * R_D;
N = R / sqrt( 1 - e2 * sin( temp2 ) * sin( temp2 ));
Space.h = sqrt( Angle.x * Angle.x + Angle.y * Angle.y ) / cos( temp2 ) - N; //高度
return ( Space ) ;
}
*/
/*********************************************************************************************/
/*程序模块：空间球坐标到空间直角坐标系下的坐标转换 */
/*函数名：Transit_to_AngleCoor() */
/*参数：Space_Point */
/*返回：空间直角坐标系下的坐标 */
/*作者： 胡锐 */
/*********************************************************************************************/
/*
Loc CTRANS::Transit_to_AngleCoor( Gps Space_Point )
{
double N ;
Loc Angle ;
N = R / sqrt( 1 - e2 * sin( Space_Point.f / R_D ) * sin( Space_Point.f / R_D ) );
//卯酉圈曲率半径
//目标点的空间直角坐标 X、Y、Z
Angle.x = ( N + Space_Point.h ) * cos ( Space_Point.f / R_D ) * cos ( Space_Point.l / R_D );
Angle.y = ( N + Space_Point.h ) * cos ( Space_Point.f / R_D ) * sin ( Space_Point.l / R_D );
Angle.z = ( N * ( 1 - e2 ) + Space_Point.h ) * sin ( Space_Point.f / R_D );
return ( Angle ) ;
}
*/
/*********************************************************************************************/
/**程序模块：计算初始投弹点的经、纬、高度及导航速度、弹体比力 **/
/**函数名：Cal_PourPoint() **/
/**参数：deltX：水平射程，deltY：垂直高度，delta：方位角， **/
/** velocity_X, velocity_Y, velocity_Z：发射坐标系下的三个方向上的速度 **/
/** phi：目标点纬度，lamda：目标点经度，h：目标点高度 **/
/** phi1：发射点纬度，lamda1：发射点经度，h1：发射点高度 **/
/** 导航系下三个速度：Nav_Vx , Nav_Vy , Nav_Vz **/
/** 弹体系下三个加速度：Bomb_Ax , Bomb_Ay , Bomb_Az **/
/**功能：由投弹点与目标点的关系反算出投弹点的经纬度坐标 **/
/*********************************************************************************************/
/*
void CTRANS::Cal_PourPoint( double *error , // deltX , deltY , delta , velocity_X , velocity_Y , velocity_Z
double *theodolite, // phi , lamda , h(目标点)
double *out_transit // phi1 , lamda1 , h1 , 导航速度 , 弹体加速度(投弹点)
)
{
int i;
// Loc Angle_TagetCoor ;
// Gps Space_TagatCoor ,TagatTransit ;
Vel Naviga_V ;
Space.f = *theodolite ; //提取目标点经、纬、高(角度、角度、米)
Space.l = *( theodolite + 1 ) ;
Space.h = *( theodolite + 2 ) ;
// Space_TagatCoor.f = TagatTransit.f ; //用于反推时存经、纬、高度
// Space_TagatCoor.l = TagatTransit.l ;
// Space_TagatCoor.h = TagatTransit.h ;
// Angle_TagetCoor = Transit_to_AngleCoor( TagatTransit ) ;//求目标点的空间直角坐标
Angle = Transit_to_AngleCoor( Space ) ;//求目标点的空间直角坐标
double deltX , deltY , delta; //提取射程、高度、初始方位角(米、米、角度)
deltX = *error ;
deltY = *( error + 1 ) ;
delta = *( error + 2 ) ;
DeltX = deltX ;
Vel Launch_V ; //提取发射点速度
Launch_V.Vx = *( error + 3 ) ;
Launch_V.Vy = *( error + 4 ) ;
Launch_V.Vz = *( error + 5 ) ;
Last_Launch_V.Vx = *( error + 3 ) ; //用于以后算速度
Last_Launch_V.Vy = *( error + 4 ) ;
Last_Launch_V.Vz = *( error + 5 ) ;
Accel Bomb_A ; //设定初始点加速度
Bomb_A.Ax = 0 ;
Bomb_A.Ay = 0 ;
Bomb_A.Az = 0 ;
double k , deltx1 , delty1;
k = ceil( sqrt( deltX *deltX + deltY * deltY ) / 50 ); //将发射和目标点等分K段
deltx1 = deltX / k;
delty1 = deltY / k;
double Alfa_TagatCoor ;
Alfa = delta / R_D ;
if ( delta > 0 )
Alfa_TagatCoor = ( delta - 180 ) / R_D ; //求变坐标原点后方位角(弧度)
else Alfa_TagatCoor = ( delta + 180 ) / R_D ;
Loc TagatCoor_P , Launch_P;
for ( i = 1 ; i <= k ; i++)
{
TagatCoor_P.x = i * deltx1 ; //目标系下空间点坐标
TagatCoor_P.y = i * delty1 ;
TagatCoor_P.z = 0 ;
Space = Calculate( Alfa_TagatCoor , TagatCoor_P , Space) ; //求经纬高
}
Launch_P.x = 0 ;
Launch_P.y = deltY ;
Launch_P.z = 0 ;
Last_Naviga = Lan_to_NavCoor( Alfa , Launch_P ) ; //求发射点导航系下坐标
// Angle = Transit_to_AngleCoor( Space_TagatCoor ) ;//计算发射点空间直角坐标为后面计算弹道点准备
Naviga_V = Lan_to_NavCoor_V( Alfa , Launch_V ) ; //发射到导航速度转换
double Delt_H ; //求真实离海拔高
Delt_H = R - sqrt( R * R - deltX * deltX ) ;
*out_transit = Space.f ;
*( out_transit + 1 ) = Space.l ;
*( out_transit + 2 ) = Space.h - Delt_H ;
// Space.f = Space_TagatCoor.f ;
// Space.l = Space_TagatCoor.l ;
// Space.h = Space_TagatCoor.h ;
*( out_transit + 3 ) = Naviga_V.Vx ;
*( out_transit + 4 ) = Naviga_V.Vy ;
*( out_transit + 5 ) = Naviga_V.Vz ;
*( out_transit + 6 ) = Bomb_A.Ax ;
*( out_transit + 7 ) = Bomb_A.Ay ;
*( out_transit + 8 ) = Bomb_A.Az ;
return ;
}
*/
/**********************************************************************************************/
/**计算时时经纬度 **/
/**函数名：coordtrans() **/
/**参数：X：发射坐标下的射程，Y：发射下的垂直直高度，Z：发射下的偏航， **/
/** velocity_X：发射下射程速度，velocity_Y：发射下垂直速度，velocity_z发射下偏航速度 **/
/** 三个欧拉角 **/
/** v_east，v_north，v_sky，导航系下的三个方向上的速度 **/
/** Bomb_accel_X , Bomb_accel_X , Bomb_accel_X , 弹体系下的三个方向上的比力 **/
/** *phi：返回的纬度，*lamda：返回的经度，*h：返回的高度 **/
/**功能：通过对传递参数的运算给出弹道点的经纬度坐标、导航系下的速度和弹体系下的比力 **/
/**********************************************************************************************/
/*
void CTRANS::coordtrans( double *launchzuobiao,//input: X , Y, Z , velocity_X , velocity_Y, velocity_z,弹体的三个欧拉角
double *returnvalue, //output: phi , lamda, h , V_east , V_north , V_sky ,
// Bomb_accel_X , Bomb_accel_Y ,Bomb_accel_Z
double interval //input: 时间间隔
)
{
Loc Launch_F ;
Vel Naviga_V , Launch_V ;
Accel Bomb_A , Launch_A , Naviga_A ,Naviga_f ;
Loc ol_angle ;
Launch_F.x = *launchzuobiao ; //提取发射系下坐标
Launch_F.y = *( launchzuobiao + 1 ) ;
Launch_F.z = *( launchzuobiao + 2 ) ;
Launch_V.Vx = *( launchzuobiao + 3 ) ; //提取发射系下三个速度
Launch_V.Vy = *( launchzuobiao + 4 ) ;
Launch_V.Vz = *( launchzuobiao + 5 ) ;
ol_angle.x = *( launchzuobiao + 6 ) ; //提取三个欧拉角
ol_angle.y = *( launchzuobiao + 7 ) ;
ol_angle.z = *( launchzuobiao + 8 ) ;
Space = Calculate( Alfa , Launch_F , Space) ; //求经纬高
Naviga_V = Lan_to_NavCoor_V( Alfa , Launch_V ) ;//求导航速度
// Launch_A.Ax = ( Launch_V.Vx - Last_Launch_V.Vx ) / interval ;//发射系下加速度
// Launch_A.Ay = ( Launch_V.Vy - Last_Launch_V.Vy ) / interval ;
// Launch_A.Az = ( Launch_V.Vz - Last_Launch_V.Vz ) / interval ;
Naviga_A.Ax = ( Naviga_V.Vx - Last_Naviga_V.Vx ) / interval ;//导航系下加速度
Naviga_A.Ay = ( Naviga_V.Vy - Last_Naviga_V.Vy ) / interval ;
Naviga_A.Az = ( Naviga_V.Vz - Last_Naviga_V.Vz ) / interval ;
// Last_Launch_V.Vx = Launch_V.Vx ;
// Last_Launch_V.Vy = Launch_V.Vy ;
// Last_Launch_V.Vz = Launch_V.Vz ;
// Bomb_A = Launch_to_Bomb_Coor_A( ol_angle , Launch_A ) ;//弹体加速度
double Delt_H ; //求真实离海拔高
Delt_H = R - sqrt( R * R - ( DeltX - Launch_F.x )* ( DeltX - Launch_F.x ) ) ;
*returnvalue = Space.f ;
*( returnvalue + 1 ) = Space.l ;
*( returnvalue + 2 ) = Space.h - Delt_H;
*( returnvalue + 3 ) = Naviga_V.Vx ;
*( returnvalue + 4 ) = Naviga_V.Vy ;
*( returnvalue + 5 ) = Naviga_V.Vz ;
double g , tempL;
// g = 9.806*(1.0-2.0*( Space.h - Delt_H ) / 6371000); //空军气象条件
tempL = sin(returnvalue[0]/R_D) *sin(returnvalue[0]/R_D) ;
g = 9.7803267714*(1+0.00527094 * tempL + 0.0000232718 * tempL*tempL)-0.000003086*returnvalue[2] ;
// Launch_A.Ax = ( Launch_V.Vx - Last_Launch_V.Vx ) / interval ;//发射系下加速度
// Launch_A.Ay -= -g ;//减去重力的发射系下加速度
// Launch_A.Az = ( Launch_V.Vz - Last_Launch_V.Vz ) / interval ;
// Last_Launch_V.Vx = Launch_V.Vx ;
// Last_Launch_V.Vy = Launch_V.Vy ;
// Last_Launch_V.Vz = Launch_V.Vz ;
Last_Naviga_V.Vx = Naviga_V.Vx ;
Last_Naviga_V.Vy = Naviga_V.Vy ;
Last_Naviga_V.Vz = Naviga_V.Vz ;
//补充
double SL, CL, Rm, Rn, w_ien[3], w_enn[3] ;
SL = sin(Space.f / R_D) ; CL = cos(Space.f / R_D) ;
Rm = Re * (1-2*Rf+3*Rf*SL*SL);//子午面曲率半径/
Rn = Re * (1+Rf*SL*SL); //卯酉面曲率半径/
w_ien[0] = 0.0; w_ien[1] = w_ie * cos(Space.f/R_D); w_ien[2] = w_ie * sin(Space.f/R_D);
w_enn[0] = -Naviga_V.Vy/(Rm+Space.h); w_enn[1] = Naviga_V.Vx/(Rn+Space.h); w_enn[2] = Naviga_V.Vx/(Rn+Space.h)*tan(Space.f/R_D);
Naviga_f.Ax = Naviga_A.Ax + ( -( 2*w_ien[2] + w_enn[2] ) ) * Naviga_V.Vy + ( 2*w_ien[1] + w_enn[1] ) * Naviga_V.Vz ;
Naviga_f.Ay = Naviga_A.Ay + ( 2*w_ien[2] + w_enn[2] ) * Naviga_V.Vx + ( -( 2*w_ien[0] + w_enn[0] ) ) * Naviga_V.Vz ;
Naviga_f.Az = Naviga_A.Az + ( -( 2*w_ien[1] + w_enn[1] ) ) * Naviga_V.Vx + ( 2*w_ien[0] + w_enn[0] ) * Naviga_V.Vy + g ;
double anb[3] = {Naviga_f.Ax, Naviga_f.Ay ,Naviga_f.Az};
double afa ,Cbn[3][3], fb[3] ;
afa = Alfa ;
void Cal_Cbn( Loc ol_angle, double afa, double Cbn[3][3] ) ; ///////////////////
Cal_Cbn( ol_angle, afa, Cbn ) ; ///////////////////
for ( int i=0; i<3; i++ )
{
fb[i] = 0;
for( int j=0; j<3; j++ )
{
fb[i] += Cbn[j][i] * anb[j] ; // 将导航系Naviga_f转到弹体系下
}
// fb[i]+=Ab[i];
}
// Bomb_A = Launch_to_Bomb_Coor_A( ol_angle , Launch_A ) ;//弹体比力输出
*( returnvalue + 6 ) = Naviga_A.Ax ;//fb[0]; // Naviga_f.Ax ; //Bomb_A.Ax ;
*( returnvalue + 7 ) = Naviga_A.Ay ;//fb[1]; //Naviga_f.Ay ; //Bomb_A.Ay ;
*( returnvalue + 8 ) = Naviga_A.Az ;//fb[2]; //Naviga_f.Az ; //Bomb_A.Az ;
*( returnvalue + 9 ) = fb[0]; //陀螺wx,wy,wz
*( returnvalue + 10 ) = fb[1];
*( returnvalue + 11 ) = fb[2];
}
*/
/**********************************************************************************************/
/**计算时时经、纬、高度 **/
/**函数名：Calculate() **/
/**参数：X：发射坐标下的射程，Y：发射下的垂直直高度，Z：发射下的偏航， **/
/** velocity_X：发射下射程速度，velocity_Y：发射下垂直速度，velocity_z发射下偏航速度 **/
/** 三个欧拉角 **/
/** v_east，v_north，v_sky，导航系下的三个方向上的速度 **/
/** Bomb_accel_X , Bomb_accel_X , Bomb_accel_X , 弹体系下的三个方向上的比力 **/
/** *phi：返回的纬度，*lamda：返回的经度，*h：返回的高度 **/
/**功能：通过对传递参数的运算给出弹道点的经纬度坐标、导航系下的速度和弹体系下的比力 **/
/**********************************************************************************************/
/*
Gps CTRANS::Calculate( double Alfa , Loc Launch_F , Gps Space)//方位角、发射坐标、前一点经纬高
{
Loc Naviga , Delt_Naviga ;
Loc Delt_Angle ;
Naviga = Lan_to_NavCoor( Alfa , Launch_F ) ;
Delt_Naviga.x = Naviga.x - Last_Naviga.x ;//导航增量
Delt_Naviga.y = Naviga.y - Last_Naviga.y ;
Delt_Naviga.z = Naviga.z - Last_Naviga.z ;
Last_Naviga.x = Naviga.x ; //存储，为后一点计算用
Last_Naviga.y = Naviga.y ;
Last_Naviga.z = Naviga.z ;
Delt_Angle = Nav_to_AngleCoor( Delt_Naviga , Space ) ;
Angle.x += Delt_Angle.x ;
Angle.y += Delt_Angle.y ;
Angle.z += Delt_Angle.z ;
Space = Angle_to_TransitCoor( Angle , Space ) ;
return ( Space ) ;
}
*/
/************************************************************************************************/
/**函数：main() **/
/**功能：调用以上两个函数 **/
/************************************************************************************************/
/*
void main(void)
{
CTRANS CONVER;
double error[6] = { 19978.8 , 10000 , -50.5, 0 , 0 , 0 } ; //deltX, deltY, delta,初始速度velocity_X,velocity_Y,velocity_Z
double theodolite[3] = { 50 , 30 , 0 } ; //目标点纬度、经度、高度
double launchzuobiao[10] = { 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 } ; //弹道时时参数,X,Y,Z,velocity_X,velocity_Y,velocity_Z,三个欧拉角
double returnvalue[9] = { 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 } ;//返回的纬经高度、东北天速度、加速度
double i =0 , j = 0 , k = 0 , l = 0 , m = 0 , interval = 0.02 ;
double kk[9] = { 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 } ;
int ii = 0 ;
FILE *fp , *fp1 , *fp2, *fp3;
CONVER.Cal_PourPoint( error , theodolite , kk ) ;
printf("%10.5f %10.5f %10.5f " , kk[0] , kk[1] , kk[2] ) ;
fp = fopen( "10000m.txt" , "r" ) ;
fp1 = fopen( "var.txt" , "w" ) ;
fp2 = fopen( "7.31.txt" , "w") ;
fp3 = fopen( "1.txt" , "w") ;
while ( !feof( fp ) )
{
ii++ ;
fscanf( fp , "%lf%lf%lf%lf%lf" , &i , &j , &k , &l , &m ) ;
launchzuobiao[0] = j ;
launchzuobiao[1] = k ;
launchzuobiao[2] = l ;
// fprintf( fp3 ,"%10.5f %10.5f %10.5f ", j , k , l ) ;
CONVER.coordtrans( launchzuobiao , returnvalue , interval ) ;
// printf("%10.7f %10.7f %10.5f\n", returnvalue[0] , returnvalue[1] , returnvalue[2] );
// fprintf( fp2 ,"%10.5f\n", Alfa * 57.3) ;
fprintf( fp3 ,"%10.5f %10.5f %10.5f\n", returnvalue[0] , returnvalue[1] , returnvalue[2] ) ;
}
fclose( fp ) ;
fclose( fp1 ) ;
fclose( fp2 ) ;
// fclose( fp3 ) ;
}
*/